import numpy as np
import matplotlib.pyplot as plt
from numpy.lib.function_base import angle
from numpy.ma.core import power
from scipy.sparse import data
from utils2 import *
import cv2


anchorImg_path = 'E:/Research/2020ContrastiveLearningForSceneLabel/Data/20210329ExperimentData/round2/anchorImgs_method2_exp2/anchorImgs'
position_path = 'E:/Research/2020ContrastiveLearningForSceneLabel/Data/20210329ExperimentData/round2/round2_sync_position.npy' # 经纬度
pkuBirdViewImg = 'D:\\Research\\2020ContrastiveLearningForSceneLabel\\Data\\pkuBirdView.png'
anchor_referenceLabel_path = 'E:/Research/2020ContrastiveLearningForSceneLabel/Data/20210329ExperimentData/round2/anchorReferenceLabel_method2_exp2.txt' # 默认没有reference label

anchor_pos_list, anchor_neg_list = getAnchorPosNegIdx3(anchorImg_path, sampleNum=16, sample_interval=2)
anchor_idx = [anchor_pos_list[i][0] for i in range(len(anchor_pos_list))]

f = open(anchor_referenceLabel_path,'r')
lines = f.readlines()
f.close()
rf_label_anchor = [int(i[0]) for i in lines]
rf_label_anchor = np.array(rf_label_anchor)

GNSS = np.load(position_path)
data_size = np.shape(GNSS)[0]
speed = np.zeros(data_size)
acceleration = np.zeros(data_size)
acc_acc = np.zeros(data_size)
angle_speed = np.zeros(data_size)

avg_range = 8

for i in range(data_size):
    if i < avg_range/2 or i > data_size - avg_range/2 - 1:
        continue
    start = int(i - avg_range/2)
    end = int(i + avg_range/2)
    dis = 0
    for j in range(start, end):
        x1 = GNSS[j][0]
        y1 = GNSS[j][1]
        x2 = GNSS[j+1][0]
        y2 = GNSS[j+1][1]
        dis += np.sqrt(np.power(x1-x2,2) + np.power(y1-y2,2))
    speed[i] = dis/(avg_range * (1/15))

for i in range(data_size):
    if i < avg_range/2 or i > data_size - avg_range/2 - 1:
        continue
    start = int(i - avg_range/2)
    end = int(i + avg_range/2)
    acceleration[i] = (speed[end] - speed[start]) / (avg_range * (1/15))

for i in range(data_size):
    if i < avg_range/2 or i > data_size - avg_range/2 - 1:
        continue
    start = int(i - avg_range/2)
    end = int(i + avg_range/2)
    delta_angle = abs(GNSS[end][2] - GNSS[start][2])
    if delta_angle > np.pi:
        delta_angle = 2*np.pi - delta_angle
    angle_speed[i] = delta_angle / (avg_range * (1/15))

angle_speed += 2

for i in range(data_size):
    if i < avg_range/2 or i > data_size - avg_range/2 - 1:
        continue
    start = int(i - avg_range/2)
    end = int(i + avg_range/2)
    acc_acc[i] = (acceleration[end] - acceleration[start]) / (avg_range * (1/15))

acc_acc -= 2

# fuck = speed
# peaks_and_troughs = get_peaks_troughs(fuck, 2)
# print('average speed ',np.average(speed))
# print('peaks and troughs number ', len(peaks_and_troughs))

cmap = 'rainbow'

plt.figure()
plt.scatter(anchor_idx, speed[anchor_idx], c=rf_label_anchor, cmap = cmap, zorder = 5) # 在速度图上画锚点
# plt.scatter(peaks_and_troughs, speed[peaks_and_troughs], c='r', zorder = 5)
plt.plot(speed) # 画速度图

plt.plot(acceleration) # 画加速度图
plt.legend(['Speed','Acceleration','Anchor', 'Peaks and Troughs'])
plt.scatter(anchor_idx, acceleration[anchor_idx], c=rf_label_anchor, cmap = cmap, zorder = 5) # 在加速度图上画锚点
plt.plot([0, data_size-1],[0,0]) # 画加速度为0的直线

plt.scatter(anchor_idx, angle_speed[anchor_idx], c=rf_label_anchor, cmap = cmap, zorder = 5) # 在速度图上画锚点
# plt.scatter(peaks_and_troughs, speed[peaks_and_troughs], c='r', zorder = 5)
plt.plot(angle_speed) # 画速度图

plt.scatter(anchor_idx, acc_acc[anchor_idx], c=rf_label_anchor, cmap = cmap, zorder = 5) # 在速度图上画锚点
# plt.scatter(peaks_and_troughs, speed[peaks_and_troughs], c='r', zorder = 5)
plt.plot(acc_acc) # 画速度图


# plt.figure()
# plt.plot(acceleration) # 画加速度
# plt.title('acceleration')

rotate = 0 # 角度制
shiftX = 625
shiftY = 620
dx = 0.777
dy = 0.777 # 以上参数都是手调的
alpha = 0.5

GNSS[:,0] = -GNSS[:,0]
GNSS[:,1] *= dx
GNSS[:,0] *= dy
GNSS[:,1] += shiftX
GNSS[:,0] += shiftY

# fig = plt.figure()
# ax = fig.add_subplot(111)
# img = gImage.imread(pkuBirdViewImg)
# img = img[0:1087,:,:]
# img[:,:,3] = alpha
# ax.imshow(img, zorder = 0)
# ax.plot(GNSS[:, 1],GNSS[:, 0], zorder = 0)
# ax.scatter(GNSS[peaks_and_troughs,1], GNSS[peaks_and_troughs,0], c = 'r', zorder = 1)


plt.show()
exit()

# 以速度曲线的波峰波谷为锚点，保存锚点照片
videoPath = 'E:/Research/2020ContrastiveLearningForSceneLabel/Data/20210329ExperimentData/round2/round2.avi'
savePath = 'E:/Research/2020ContrastiveLearningForSceneLabel/Data/20210329ExperimentData/round2/anchorImgs_speedPeaksAndTroughs'
cap = cv2.VideoCapture(videoPath)
anchorImgPath = os.path.join(savePath, 'anchorImgs') # 保存的是锚点图像
if not os.path.exists(anchorImgPath):
    os.makedirs(anchorImgPath)
else:
    print('Path %s already exist! Program exit.'% anchorImgPath)
    exit(-1)

for fno in peaks_and_troughs:
    cap.set(cv2.CAP_PROP_POS_FRAMES, fno)
    if cap.get(cv2.CAP_PROP_POS_FRAMES) != fno:
        print('Frame number error! Program exit.')
        exit(-1)
    ret, frame = cap.read()
    if not ret:
        print('Frame read error! Program exit.')
        exit(-1)
    imgPath = str(int(fno)).rjust(10,'0') + '.png'
    imgPath = os.path.join(anchorImgPath, imgPath)
    res = cv2.imwrite(imgPath, frame)
    if res:
        print('Write anchor image to %s'%imgPath)
    else:
        raise ValueError('Error!!! Can not write anchor image to %s'%imgPath)
cap.release()
plt.show()
exit(0)